/**
 * This file is part of Fast-Planner.
 *
 * Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of
 * Science and Technology, <uav.ust.hk> Developed by Boyu Zhou <bzhouai at
 * connect dot ust dot hk>, <uv dot boyuzhou at gmail dot com> for more
 * information see <https://github.com/HKUST-Aerial-Robotics/Fast-Planner>. If
 * you use this code, please cite the respective publications as listed on the
 * above website.
 *
 * Fast-Planner is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Fast-Planner is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with Fast-Planner. If not, see <http://www.gnu.org/licenses/>.
 */

#include "visualization_msgs/Marker.h"
#include <ros/ros.h>

#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <random>
#include <sensor_msgs/PointCloud2.h>
#include <string>

#include <plan_env/linear_obj_model.hpp>
using namespace std;

int obj_num, _input_type;
double _x_size, _y_size, _h_size, _vel, _yaw_dot, _acc_r1, _acc_r2, _acc_z,
    _scale1, _scale2, _interval;

ros::Publisher obj_pub;           // visualize marker
vector<ros::Publisher> pose_pubs; // obj pose (from optitrack)
vector<LinearObjModel> obj_models;

random_device rd;
default_random_engine eng(rd());
uniform_real_distribution<double> rand_pos_x;
uniform_real_distribution<double> rand_pos_y;
uniform_real_distribution<double> rand_h;
uniform_real_distribution<double> rand_vel;
uniform_real_distribution<double> rand_acc_r;
uniform_real_distribution<double> rand_acc_t;
uniform_real_distribution<double> rand_acc_z;
uniform_real_distribution<double> rand_color;
uniform_real_distribution<double> rand_scale;
uniform_real_distribution<double> rand_yaw_dot;
uniform_real_distribution<double> rand_yaw;

ros::Time time_update, time_change;

void updateCallback(const ros::TimerEvent &e);
void visualizeObj(int id);

int main(int argc, char **argv) {
    ros::init(argc, argv, "dynamic_obj");
    ros::NodeHandle node("~");

    /* ---------- initialize ---------- */
    node.param("obj_generator/obj_num", obj_num, 20);
    node.param("obj_generator/x_size", _x_size, 10.0);
    node.param("obj_generator/y_size", _y_size, 10.0);
    node.param("obj_generator/h_size", _h_size, 2.0);
    node.param("obj_generator/vel", _vel, 2.0);
    node.param("obj_generator/yaw_dot", _yaw_dot, 2.0);
    node.param("obj_generator/acc_r1", _acc_r1, 2.0);
    node.param("obj_generator/acc_r2", _acc_r2, 2.0);
    node.param("obj_generator/acc_z", _acc_z, 0.0);
    node.param("obj_generator/scale1", _scale1, 0.5);
    node.param("obj_generator/scale2", _scale2, 1.0);
    node.param("obj_generator/interval", _interval, 100.0);
    node.param("obj_generator/input_type", _input_type, 1);

    obj_pub = node.advertise<visualization_msgs::Marker>("/dynamic/obj", 10);
    for (int i = 0; i < obj_num; ++i) {
        ros::Publisher pose_pub = node.advertise<geometry_msgs::PoseStamped>(
            "/dynamic/pose_" + to_string(i), 10);
        pose_pubs.push_back(pose_pub);
    }

    ros::Timer update_timer =
        node.createTimer(ros::Duration(1 / 30.0), updateCallback);
    cout << "[dynamic]: initialize with " + to_string(obj_num) << " moving obj."
         << endl;
    ros::Duration(1.0).sleep();

    rand_color = uniform_real_distribution<double>(0.0, 1.0);
    rand_pos_x = uniform_real_distribution<double>(-_x_size / 2, _x_size / 2);
    rand_pos_y = uniform_real_distribution<double>(-_y_size / 2, _y_size / 2);
    rand_h = uniform_real_distribution<double>(0.0, _h_size);
    rand_vel = uniform_real_distribution<double>(-_vel, _vel);
    rand_acc_t = uniform_real_distribution<double>(0.0, 6.28);
    rand_acc_r = uniform_real_distribution<double>(_acc_r1, _acc_r2);
    rand_acc_z = uniform_real_distribution<double>(-_acc_z, _acc_z);
    rand_scale = uniform_real_distribution<double>(_scale1, _scale2);
    rand_yaw = uniform_real_distribution<double>(0.0, 2 * 3.141592);
    rand_yaw_dot = uniform_real_distribution<double>(-_yaw_dot, _yaw_dot);

    /* ---------- give initial value of each obj ---------- */
    for (int i = 0; i < obj_num; ++i) {
        LinearObjModel model;
        Eigen::Vector3d pos(rand_pos_x(eng), rand_pos_y(eng), rand_h(eng));
        Eigen::Vector3d vel(rand_vel(eng), rand_vel(eng), 0.0);
        Eigen::Vector3d color(rand_color(eng), rand_color(eng),
                              rand_color(eng));
        Eigen::Vector3d scale(rand_scale(eng), 1.5 * rand_scale(eng),
                              5.0 * rand_scale(eng));
        double yaw = rand_yaw(eng);
        double yaw_dot = rand_yaw_dot(eng);

        double r, t, z;
        r = rand_acc_r(eng);
        t = rand_acc_t(eng);
        z = rand_acc_z(eng);
        Eigen::Vector3d acc(r * cos(t), r * sin(t), z);

        if (_input_type == 1) {
            model.initialize(pos, vel, acc, yaw, yaw_dot, color, scale,
                             _input_type); // Vel input
        } else {
            model.initialize(pos, Eigen::Vector3d(0, 0, 0), acc, yaw, yaw_dot,
                             color, scale, _input_type); // Acc input
        }
        model.setLimits(Eigen::Vector3d(_x_size / 2, _y_size / 2, _h_size),
                        Eigen::Vector2d(0.0, _vel), Eigen::Vector2d(0, 0));
        obj_models.push_back(model);
    }

    time_update = ros::Time::now();
    time_change = ros::Time::now();

    /* ---------- start loop ---------- */
    ros::spin();

    return 0;
}

void updateCallback(const ros::TimerEvent &e) {
    ros::Time time_now = ros::Time::now();

    /* ---------- change input ---------- */
    // double dtc = (time_now - time_change).toSec();
    // if (dtc > _interval) {
    //   for (int i = 0; i < obj_num; ++i) {
    //     /* ---------- use acc input ---------- */
    //     // double r, t, z;
    //     // r = rand_acc_r(eng);
    //     // t = rand_acc_t(eng);
    //     // z = rand_acc_z(eng);
    //     // Eigen::Vector3d acc(r * cos(t), r * sin(t), z);
    //     // obj_models[i].setInput(acc);

    //     /* ---------- use vel input ---------- */
    //     double vx, vy, vz, yd;
    //     vx = rand_vel(eng);
    //     vy = rand_vel(eng);
    //     vz = 0.0;
    //     yd = rand_yaw_dot(eng);

    //     obj_models[i].setInput(Eigen::Vector3d(vx, vy, vz));
    //     obj_models[i].setYawDot(yd);
    //   }
    //   time_change = time_now;
    // }

    /* ---------- update obj state ---------- */
    double dt = (time_now - time_update).toSec();
    time_update = time_now;
    for (int i = 0; i < obj_num; ++i) {
        obj_models[i].update(dt);
        visualizeObj(i);
        ros::Duration(0.000001).sleep();
    }

    /* ---------- collision ---------- */
    // for (int i = 0; i < obj_num; ++i)
    //   for (int j = i + 1; j < obj_num; ++j) {
    //     bool collision = LinearObjModel::collide(obj_models[i],
    //     obj_models[j]); if (collision) {
    //       double yd1 = rand_yaw_dot(eng);
    //       double yd2 = rand_yaw_dot(eng);
    //       obj_models[i].setYawDot(yd1);
    //       obj_models[j].setYawDot(yd2);
    //     }
    //   }
}

void visualizeObj(int id) {
    Eigen::Vector3d pos, color, scale;
    pos = obj_models[id].getPosition();
    color = obj_models[id].getColor();
    scale = obj_models[id].getScale();
    double yaw = obj_models[id].getYaw();

    Eigen::Matrix3d rot;
    rot << cos(yaw), -sin(yaw), 0.0, sin(yaw), cos(yaw), 0.0, 0.0, 0.0, 1.0;

    Eigen::Quaterniond qua;
    qua = rot;

    /* ---------- rviz ---------- */
    visualization_msgs::Marker mk;
    mk.header.frame_id = "world";
    mk.header.stamp = ros::Time::now();
    mk.type = visualization_msgs::Marker::CUBE;
    mk.action = visualization_msgs::Marker::ADD;
    mk.id = id;

    mk.scale.x = scale(0), mk.scale.y = scale(1), mk.scale.z = scale(2);
    mk.color.a = 1.0, mk.color.r = color(0), mk.color.g = color(1),
    mk.color.b = color(2);

    mk.pose.orientation.w = qua.w();
    mk.pose.orientation.x = qua.x();
    mk.pose.orientation.y = qua.y();
    mk.pose.orientation.z = qua.z();

    mk.pose.position.x = pos(0), mk.pose.position.y = pos(1),
    mk.pose.position.z = pos(2);

    obj_pub.publish(mk);

    /* ---------- pose ---------- */
    geometry_msgs::PoseStamped pose;
    pose.header.frame_id = "world";
    pose.header.seq = id;
    pose.pose.position.x = pos(0), pose.pose.position.y = pos(1),
    pose.pose.position.z = pos(2);
    pose.pose.orientation.w = 1.0;
    pose_pubs[id].publish(pose);
}
